function writeSpringCartPoleDynamics(f,M)

%This function writes the dynamics file for the double pendulum.

filename = 'springCartPoleDynamics';

comments{1} = ['DZ = ' upper(filename) '(T,Z,P)'];
comments{2} = ' ';
comments{3} = 'FUNCTION:  This function computes the dynamics of a spring';
comments{4} = '    cart-pole, and is designed to be called from ode45. The';
comments{5} = '    model uses a point-mass cart and pendulum bob, with no';
comments{6} = '    no friction or actuation';
comments{7} = ' ';
comments{8}  = 'INPUTS: ';
comments{9}  = '    t = time. Dummy input for ode45. Not used.';
comments{10} = '    z = [4xn] matrix of states.';
comments{12} = '    P = struct of parameters';
comments{13} = 'OUTPUTS: ';
comments{14} = '    dz = [4xn] matrix of state derivatives';
comments{15} = ' ';
comments{16} = 'NOTES:';
comments{17} = ['    This file was automatically generated by ' mfilename]; 

params{1} = {'m1','cart mass'};
params{2} = {'m2','bob mass'};
params{3} = {'g ','gravity'};
params{4} = {'l','pendulum length'};
params{5} = {'k','spring constant'};

states{1} = {'x','link one absolute angle'};
states{2} = {'th','link one angular rate'};
states{3} = {'dx','link two absolute angle'};
states{4} = {'dth','link two angular rate'};

dstates{1} = {'dx','derivative of link one absolute angle'};
dstates{2} = {'dth','derivative of link one angular rate'};
dstates{3} = {'ddx','derivative of link two absolute angle'};
dstates{4} = {'ddth','derivative of link two angular rate'};

%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~%
%                               write file                                %
%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~%
fid = fopen([filename '.m'],'w');

fprintf(fid, ['function dz = ' filename '(~,z,P) \n']);

for i=1:length(comments)
    fprintf(fid,['%%' comments{i} '\n']);
end
fprintf(fid,'\n');

for i=1:length(params)
    fprintf(fid,[params{i}{1} ' = P.' params{i}{1} '; %%' params{i}{2} '\n']);
end
fprintf(fid,'\n');

for i=1:length(states)
    fprintf(fid,[states{i}{1} ' = z(' num2str(i) ',:); %%' states{i}{2} '\n']);
end
fprintf(fid,'\n');

for i=1:2;
    fprintf(fid,['f' num2str(i) ' = ' vectorize(char(f(i))) ';\n']);
end
fprintf(fid,'\n');

for i=1:2
    for j=1:2
        fprintf(fid,['M' num2str(i) num2str(j) ' = ' vectorize(char(M(i,j))) ';\n']);
    end
end
fprintf(fid,'\n');

fprintf(fid,'D = M11.*M22 - M12.*M21;\n\n');

fprintf(fid,'ddx = (f2.*M12 - f1.*M22)./D;\n');
fprintf(fid,'ddth = -(f2.*M11 - f1.*M21)./D;\n');
fprintf(fid,'\n');

fprintf(fid,'dz = [...\n');
for i=1:length(dstates)
    fprintf(fid,['    ' dstates{i}{1} '; %%' dstates{i}{2} '\n']);
end
fprintf(fid,'];\n');

fprintf(fid,'end \n');

fclose(fid);
end


